
import lejos.nxt.*;
import java.util.*;

public class MotorControl {
	
	// Brightness threshold 
	private int threshold = 0;

	// Array containing scan hits
	public ArrayList<Long> scanHits = new ArrayList<Long>();
	
	// Class constructor
	public MotorControl(int getThreshold) {
		threshold = getThreshold;
	}
	
	/**
	 * General motor control
	 * Starts and stops every motor, set speed etc.
	 * It also creates an light sensor object 
	 */
	public void startMotor(int motor, int direction, int speed, long runtime, int wait) {
		scanHits = new ArrayList<Long>();
		wait = wait * 1000;
		long t = System.currentTimeMillis();
		long end = t+runtime;
		int start = 0;
		int hit = 0;
		long currentTime = 0;
		long lastScanHit = 0;
		
		// Create a new light sensor object used to track changes of brightness
		LightSensor lightSensor = new LightSensor(SensorPort.S2);
		
		while(System.currentTimeMillis() < end) {
			currentTime = runtime - (end - System.currentTimeMillis());
			
			// Make readings visible on NXT
			LCD.drawInt(lightSensor.getLightValue(), 4, 0, 0);

			if(lightSensor.getLightValue() < threshold) {
				// Register scan hits and make sure we don't duplicate hits
				if((currentTime - lastScanHit) > 500) {
					addScanHit(currentTime);
					lastScanHit = currentTime;
				}
				if(hit == 0) {
					hit = 1;
					LCD.clear();
					LCD.drawString("Found something!", 0, 1);
				}
			} else {
				if(hit == 1) {
					hit = 0;
					LCD.clear();
					LCD.drawString("Searching..", 0, 1);
				}
			}
			
			// Figure out which motor to start
			if(start == 0) {
				switch(motor) {
					case 1: 
						if(direction == 1) { 
							Motor.A.forward(); 
						} else {
							Motor.A.backward();
						}
						Motor.A.setSpeed(speed);
						break;
					case 2: 
						if(direction == 1) { 
							Motor.B.forward(); 
						} else {
							Motor.B.backward();
						}
						Motor.B.setSpeed(speed);
						break;
					case 3: 
						if(direction == 1) { 
							Motor.C.forward(); 
						} else {
							Motor.C.backward();
						}
						Motor.C.setSpeed(speed);
						break;
				}
				start = 1;
			}
		}
		
		start = 0;
		t = System.currentTimeMillis();
		end = t+wait;
		
		//..and which motor to stop, then wait n seconds
		while(System.currentTimeMillis() < end) {
			if(start == 0) {
				switch(motor) {
					case 1: 
						Motor.A.stop();
						break;
					case 2: 
						Motor.B.stop();
						break;
					case 3: 
						Motor.C.stop();
						break;
				}
				start = 1;
			}
		}
	} 
	
	// Add scan hits to our array
	public void addScanHit(long timestamp) {
		scanHits.add(timestamp);
	}
	
	// Retrieve scan hits array
	public ArrayList<Long> getScanHits() {
		return scanHits;
	}
}
